
#include "LSM303DLH.h"


void LSM303DLH_Init(void )
{
	LSM_Acc_ConfigTypeDef  LSM_Acc_InitStructure;
	LSM_Acc_Filter_ConfigTypeDef LSM_Acc_FilterStructure;
	LSM_Magn_ConfigTypeDef LSM_Magn_InitStructure;

	LSM303DLH_I2C_Init();

	
	LSM_Acc_InitStructure.Power_Mode = LSM_Acc_Lowpower_NormalMode;
	LSM_Acc_InitStructure.ODR = LSM_Acc_ODR_50;
	LSM_Acc_InitStructure.Axes_Enable= LSM_Acc_XYZEN;
	LSM_Acc_InitStructure.FS = LSM_Acc_FS_2;
	LSM_Acc_InitStructure.Data_Update = LSM_Acc_BDU_Continuos;
	LSM_Acc_InitStructure.Endianess=LSM_Acc_Big_Endian;
	
	LSM_Acc_FilterStructure.HPF_Enable=LSM_Acc_Filter_Disable;
	LSM_Acc_FilterStructure.HPF_Mode=LSM_Acc_FilterMode_Normal;
	LSM_Acc_FilterStructure.HPF_Reference=0x00;
	LSM_Acc_FilterStructure.HPF_Frequency=LSM_Acc_Filter_HPc16;
	
	LSM303DLH_Acc_Config(&LSM_Acc_InitStructure);
	LSM303DLH_Acc_Filter_Config(&LSM_Acc_FilterStructure);		
	
	LSM_Magn_InitStructure.M_ODR = LSM_Magn_ODR_30;
	LSM_Magn_InitStructure.Meas_Conf = LSM_Magn_MEASCONF_NORMAL;
	LSM_Magn_InitStructure.Gain = LSM_Magn_GAIN_1_3;
	LSM_Magn_InitStructure.Mode = LSM_Magn_MODE_CONTINUOS ;
	LSM303DLH_Magn_Config(&LSM_Magn_InitStructure);			
}



/**
* @brief  Change a a configuration parameter of a iNEMO sensor
* @param pData : poiter to iNEMO_data structure
* @param parameter : paramenter to change  <ul>
 * <li>0x00 - Output data rate</li>
 * <li>0x01 Full Scale </li>
 * <li>0x02 HPF  </li>
 * <li>0x03 Offset X </li>
 * <li>0x04 Offset Y </li>
 * <li>0x05 Offset Z </li>
 * </ul>  
 * @ulenght : Lenght of value payload
 * @param value : new value to set
*/  
bool iNEMO_Acc_Set_Config( iNEMO_DATA * pdata, u8 uparameter, u8 ulenght, u8* pvalue)  
  {
    bool isAllowed=FALSE; 
    
    switch(uparameter)
    {
    case 0x00: /*!< set ODR */
     
      if(pvalue[0] < 0x04 && ulenght == 2)
      {   
        u8 uCtrl=0;;
        LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl, LSM_A_CTRL_REG1_ADDR, 1);
        uCtrl = (uCtrl & 0xE7) + (pvalue[0] << 3);  
        LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &uCtrl, LSM_A_CTRL_REG1_ADDR);
        LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl, LSM_A_CTRL_REG1_ADDR, 1);
        /* check if the command has been executed */
        if(((uCtrl >> 3) & 0x03) == pvalue[0])
          /* if yes  send ack  */  
          isAllowed=TRUE;
        else
          /* else send nack  */              
          isAllowed=FALSE;    
      }
      else
      {
        isAllowed=FALSE;
      }
      break;    
      
    case  0x01:  /*!< Set Full scale*/
      
      if(pvalue[0] < 0x04 &&  pvalue[0] != 0x02  && ulenght == 2 )
      {
        u8 uCtrl=0;
        LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl, LSM_A_CTRL_REG4_ADDR, 1);
        uCtrl =(uCtrl & 0xCF) + ((pvalue[0] & 0x03) << 4);  
        LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &uCtrl, LSM_A_CTRL_REG4_ADDR);
        LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl, LSM_A_CTRL_REG4_ADDR, 1);
        /* check if the command has been executed */        
        if(((uCtrl & 0x30) >> 4 ) == pvalue[0]) 
          /* if yes   */            
        {
          switch(pvalue[0])  
          {
            /* update Gain Value*/
          case 0x00:
              iNEMO_GainAccX(pdata)= LSM_Acc_Sensitivity_2g;
              iNEMO_GainAccY(pdata)= LSM_Acc_Sensitivity_2g;
              iNEMO_GainAccZ(pdata)= LSM_Acc_Sensitivity_2g;
          break;
          
          case 0x01:
              iNEMO_GainAccX(pdata)= LSM_Acc_Sensitivity_4g;
              iNEMO_GainAccY(pdata)= LSM_Acc_Sensitivity_4g;
              iNEMO_GainAccZ(pdata)= LSM_Acc_Sensitivity_4g;
              break;

          case 0x03:
              iNEMO_GainAccX(pdata)= LSM_Acc_Sensitivity_8g;
              iNEMO_GainAccY(pdata)= LSM_Acc_Sensitivity_8g;
              iNEMO_GainAccZ(pdata)= LSM_Acc_Sensitivity_8g; 
              break;
          }        
       
          isAllowed=TRUE;
        }
        else
          /* else  send nack  */              
          isAllowed=FALSE;
      }
      else
        isAllowed=FALSE;
      break;   
      
    case  0x02:  /* HPF enable*/
      
           if(ulenght == 3)  
          { 
            
            u8 temp=pvalue[0];
            /* 
            pvalue[0] = | RFU |RFU |Filter Enable/Disable |REF ENABLE/DISABLE |HP1 |HP0 | ODR1 |ODR0
            */
            if((temp & 0x20) == 0x20)  /* check filter ENABLE command */  
            {   
              u8 uCtrl1, uCtrl2, temp2, ref;
              LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl1, LSM_A_CTRL_REG1_ADDR, 1); 
              uCtrl1 =  (uCtrl1 & 0xE7) + ((temp  & 0x03) << 3);  //set ODR
              LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &uCtrl1, LSM_A_CTRL_REG1_ADDR);/*!<set the ODR value in the ctrl1 register*/
              LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl1, LSM_A_CTRL_REG1_ADDR, 1);  
              
              if((temp & 0x10) == 0x10)  /* check if reference is enabled */
              {
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &temp2, LSM_A_CTRL_REG2_ADDR, 1);
                uCtrl2 = ((temp2  & 0xCC) + ((temp  & 0x0C) >> 2) + 0x30); /*  !<set cut off frequency and bits to enble filter and enable reference */   
                LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR);
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR, 1);  
                LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &pvalue[1], LSM_A_REFERENCE_REG_ADDR); /*!<WRITE the refence value in the LSM_A_REFERENCE_REG_ADDR register*/        
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &ref, LSM_A_REFERENCE_REG_ADDR,1);    
                temp2= ((uCtrl2 & 0x03) <<2) + (uCtrl2 & 0x30);
                temp2 = (temp2 & 0xFC) + ((uCtrl1 & 0x18) >> 3); 
                if((temp2 == temp)  && (ref==pvalue[1])) /*!< check if the command has been executed && ((uCtrl2 & 0x10) == 0x10)*/         
                 isAllowed=TRUE;
                else
                isAllowed=FALSE;           
              }
              else
              { 
                
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR, 1);
                uCtrl2 = ((temp2  & 0xCC) + ((temp  & 0x0C) >> 2) + 0x10);  /*!< set cut off frequency and set to 1 FDS bit to enble filter */            
                LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR);
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR, 1);  
                uCtrl1 = ((uCtrl1 & 0x18) >> 3);
                uCtrl1 = uCtrl1  + ((uCtrl2 & 0x03) <<2) ;
                uCtrl1 = uCtrl1  +  ((uCtrl2 & 0x10)<<1);
                if((uCtrl1 == temp ) && ((uCtrl2 & 0x10) == 0x10)) /*!< check if the command has been executed*/     
                  isAllowed=TRUE;
                else
                  isAllowed=FALSE;               
              }  
            }
            else /* filter disable*/
            {
                u8 uCtrl2;
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR, 1);
                uCtrl2 = (uCtrl2  & 0xCF);  /* !< set to 0 FDS bit to disable filter */            
                LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR);  
                LSM303DLH_I2C_ByteWrite(LSM_A_I2C_ADDRESS, 0x00, LSM_A_REFERENCE_REG_ADDR);
                LSM303DLH_I2C_BufferRead(LSM_A_I2C_ADDRESS, &uCtrl2, LSM_A_CTRL_REG2_ADDR, 1);
                if((uCtrl2  & 0x10) == 0x00)
                   isAllowed=TRUE;
                else
                   isAllowed=FALSE;                    
            }
              
    }
    break;
    
    case 0x03: /* !<  set x axis offset*/
      if(ulenght == 3 )
      {
      iNEMO_OffAccX(pdata)=((s16)pvalue[0] << 8) + pvalue[1];
      isAllowed=TRUE;
      }
      else
        isAllowed=FALSE;
        
      break;
      
    case 0x04: /*!<  set y axis offset*/
      if(ulenght == 3 )
      {
      iNEMO_OffAccY(pdata)=((s16)pvalue[0] << 8) + pvalue[1];
      isAllowed=TRUE;
      }
      else
        isAllowed=FALSE;
        
      break;
      
    case 0x05: /*!< set z axis offset*/
      if(ulenght == 3 )
      {
      iNEMO_OffAccZ(pdata)=((s16)pvalue[0] << 8) + pvalue[1];
      isAllowed=TRUE;
      }
      else
        isAllowed=FALSE;
      break;
 
  }
  return isAllowed;
  }



/**
* @brief  Change a  configuration parameter of a iNEMO magnetometer
* @param parameter : paramenter to change  <ul>
 * <li>0x00 - Output data rate</li>
 * <li>0x01 Full Scale </li>
 * <li>0x02 MAGN operationg mode </li>
 * <li>0x03 Offset X </li>
 * <li>0x04 Offset Y </li>
 * <li>0x05 Offset Z </li>
 * </ul>  
* @param value : new value to set
*/  
  
bool iNEMO_Magn_Set_Config( iNEMO_DATA * pdata, u8 uparameter, u8 ulenght, u8* pvalue)  
  {
    bool isAllowed=FALSE;
    switch(uparameter)
    {
    case 0x00: /*!<  set ODR */
      if(pvalue[0] < 0x07 && ulenght == 2)
      {
        u8 uCtrl;
        LSM303DLH_I2C_BufferRead(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRA_REG_ADDR, 1);      
        uCtrl = (uCtrl & 0xE3) + (pvalue[0]  << 2);  
        LSM303DLH_I2C_ByteWrite(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRA_REG_ADDR);
        LSM303DLH_I2C_BufferRead(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRA_REG_ADDR, 1);
        /* check if the command has been executed */
        if(((uCtrl >>2)  & 0x07) == pvalue[0]) 
          /* if yes  send ack  */  
          isAllowed=TRUE;
        else
          /* else send nack  */              
          isAllowed=FALSE;
        
      }
      else
      {
        isAllowed=FALSE;
      }
      break;    
      
    case  0x01:  /*!< Set Full scale*/
      
      if(pvalue[0] < 0x08 &&  pvalue[0] > 0x00  && ulenght == 2 )
      {
        u8 uCtrl;
        uCtrl =(pvalue[0] << 5) & 0xE0;  
        LSM303DLH_I2C_ByteWrite(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRB_REG_ADDR);
        LSM303DLH_I2C_BufferRead(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRB_REG_ADDR, 1);
        /* check if the command has been executed */
        if(((uCtrl >>5)  & 0x07) == pvalue[0])         
        {
          switch(pvalue[0])  
          {
          case 0x01:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_1_3Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_1_3Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_XY_1_3Ga;
            break;
            
          case 0x02:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_1_9Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_1_9Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_Z_1_9Ga;
            break;
            
          case 0x03:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_2_5Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_2_5Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_Z_2_5Ga; 
            break;
            
          case 0x04:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_4Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_4Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_Z_4Ga; 
            break;   
            
          case 0x05:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_4_7Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_4_7Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_Z_4_7Ga; 
            break;          
            
          case 0x06:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_5_6Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_5_6Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_Z_5_6Ga; 
            break;  

          case 0x07:
            iNEMO_GainMagnX(pdata)= LSM_Magn_Sensitivity_XY_8_1Ga;
            iNEMO_GainMagnY(pdata)= LSM_Magn_Sensitivity_XY_8_1Ga;
            iNEMO_GainMagnZ(pdata)= LSM_Magn_Sensitivity_Z_8_1Ga; 
            break;             
            
          }        
          
          isAllowed=TRUE;
        }
        else
          /* else  send nack  */              
          isAllowed=FALSE;
      }
      else
        isAllowed=FALSE;
      break;   
      
    case  0x02:  /*!<  Magn operating mode*/
      
      if(pvalue[0] < 0x03 && ulenght == 2)
      {
        u8 uCtrl;
        LSM303DLH_I2C_BufferRead(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRA_REG_ADDR, 1);
        uCtrl = (uCtrl & 0xFC) + pvalue[0];  
        LSM303DLH_I2C_ByteWrite(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRA_REG_ADDR);
        LSM303DLH_I2C_BufferRead(LSM_M_I2C_ADDRESS, &uCtrl, LSM_M_CRA_REG_ADDR, 1);
        /* check if the command has been executed */
        if(((uCtrl & 0x03) == pvalue[0])) 
          /* if yes  send ack  */  
          isAllowed=TRUE;
        else
          /* else send nack  */              
          isAllowed=FALSE;
        
      }
      else
      {
        isAllowed=FALSE;
      }
      break; 
    
    case 0x03: /*!< set x axis offset*/
      if(ulenght == 3 )
      {
      iNEMO_OffMagnX(pdata)=((s16)pvalue[0] << 8) + pvalue[1];
      isAllowed=TRUE;
      }
      else
        isAllowed=FALSE;
        
      break;
      
    case 0x04: /*!<  set y axis offset*/
      if(ulenght == 3 )
      {
      iNEMO_OffMagnY(pdata)=((s16)pvalue[0] << 8) + pvalue[1];
      isAllowed=TRUE;
      }
      else
        isAllowed=FALSE;
        
      break;
      
    case 0x05: /*!<  set z axis offset*/
      if(ulenght == 3 )
      {
      iNEMO_OffMagnZ(pdata)=((s16)pvalue[0] << 8) + pvalue[1];
      isAllowed=TRUE;
      }
      else
        isAllowed=FALSE;
      break;
 
  }
  return isAllowed;
}